// CubicQuartic
//  Cubic and quartic equations are the highest order polynomial equations 
//  that can be solved by radicals in the general case.
// Created in 1998-99 by Aloudin (Albert) Akhriev.

#pragma once

#include <limits>
#include "pi.h"

namespace g3 {
	template <class T> class basic_smatrix;
	template <class T> class basic_matrix;
}

//! ROOT FINDING. 
/*!
 Returns true and places in Root1 and Root2 the roots of the
 quadratic polynomial A*t*t+B*t+C (where A is presumed nonzero),
 iff the polynomial has a non-negative discriminant; in this case
 Root1 is guaranteed to be smaller than or equal to Root2. Iff the
 polynomial has a negative discriminant, it returns false and leaves
 Root1 and Root2 unchanged. 
*/
template <class T> 
inline bool findRoots(T A, T B, T C, T &Root1, T &Root2) 
              { const T D=sqr(B)-(4*A*C); if (D<0) return false;
                if (B==0) { Root2=sqrt(-C/A); Root1=-Root2; return true; }
                const T Q=T(-0.5)*((B>0)?(B+sqrt(D)):(B-sqrt(D)));
                if ((B>0)^(A>0)) { Root1=C/Q; Root2=Q/A; }
                else { Root1=Q/A; Root2=C/Q; }
                return true; }

// The function finds real roots of cubic equation.
// Equation: x^3 + a*x^2 + b*x + c = 0.
// Parameters: a, b, c - coefficients of cubic equation;
//             root    - pointer to the output root[] array;
// Returns: number of real roots (1 or 3).
template <typename T>
int cubic(T a, T b, T c, T root[3])
{
  T Q = (a*a - 3.0*b) * (1.0/9.0);
  T R = (2.0*a*a*a - 9.0*a*b + 27.0*c) * (1.0/54.0);
  T cub_Q = Q*Q*Q;
  T sqr_R = R*R;
  T a_div_3 = a * (1.0/3.0);

  if (sqr_R*(1.0-4.0*std::numeric_limits<T>::epsilon()) <= cub_Q) // sqr_R >= 0  ==>  cub_Q >= 0
  {
    T sqrt_Q = sqrt(Q);
    T ratio = R/(Q*sqrt_Q);
    T angle = (1.0/3.0) * acos((ratio < 1.0) ? ratio : 1.0);
    T A = -2.0*sqrt_Q;
    root[0] = A * cos(angle) - a_div_3;    
    root[1] = A * cos(angle + (2.0*PI/3.0)) - a_div_3; 
    root[2] = A * cos(angle - (2.0*PI/3.0)) - a_div_3;
    return 3;
  }
  else
  {
    int sign_R = (R > 0.0) ? +1 : ((R < 0.0) ? -1 : 0);
    T A = -sign_R * pow(fabs(R) + sqrt(fabs(sqr_R-cub_Q)), (1.0/3.0));
    T B = (A != 0.0) ? Q/A : 0.0;
    root[0] = (A+B) - a_div_3;
    return 1;
  }
}


// The function finds real roots of quartic equation. Ferrari's solution.
// Equation: x^4 + a*x^3 + b*x^2 + c*x + d = 0.
// Parameters: a, b, c, d - coefficients of quartic equation;
//             root       - pointer to the output root[] array;
// Returns: number of real roots (0 or 2 or 4).
template <typename T>
int quartic(T a, T b, T c, T d, T root[4])
{
  T cubicRoot[3];
  cubic(-b, a*c-4*d, -a*a*d+4*b*d-c*c, cubicRoot);
  T A = 0.25*a*a - b + cubicRoot[0];
  T B = 0.5*a*cubicRoot[0] - c;
  const T EPSILON2 = std::numeric_limits<T>::epsilon() * std::numeric_limits<T>::epsilon();
  if (A < -EPSILON2)
    return 0;
  if (A > +EPSILON2)
  {
    A = sqrt(A);
    B = B/(2.0*A);
  }
  else
  {
    A = 0.0;
    B = 0.25*cubicRoot[0]*cubicRoot[0] - d;
    if (B < -EPSILON2)
      return 0;
    B = (B > +EPSILON2) ? sqrt(B) : 0.0;
  }

  int num = 0;  
  if (findRoots(1.0, 0.5*a+A, 0.5*cubicRoot[0]+B, root[num], root[num+1]))
    num += 2;
  if (findRoots(1.0, 0.5*a-A, 0.5*cubicRoot[0]-B, root[num], root[num+1]))
    num += 2;
  return num;
}

// The function finds real roots of quartic equation in the form:
// [x^2 x 1] S [x^2 x 1]^T = 0.
// Returns: number of real roots (0 or 2 or 4).
template <typename T>
inline int quartic(const g3::basic_matrix<T> & s, T root[4])
{
  if (s.d_e00 == 0)
  {
    assert (false);
    return 0;
  }
  T r = 1 / s.d_e00;
  return quartic(
    (s.d_e01 + s.d_e10) * r,
    (s.d_e02 + s.d_e20 + s.d_e11) * r,
    (s.d_e12 + s.d_e21) * r,
    s.d_e22 * r, 
    root);
}

template <typename T>
inline int quartic(const g3::basic_smatrix<T> & s, T root[4])
{
  if (s.xx == 0)
  {
    assert (false);
    return 0;
  }
  T r = 1 / s.xx;
  return quartic(
    2 * s.xy * r,
    (2 * s.xz + s.yy) * r,
    2 * s.yz * r,
    s.zz * r, 
    root);
}
